#!/usr/bin/env python3
"""
Vision Status Manager - 视觉系统状态管理器
管理视觉交互系统的状态同步和错误传播
"""

import time
import threading
from typing import Dict, Any, List, Optional, Callable
from enum import Enum
from dataclasses import dataclass, field
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from std_msgs.msg import String
import json


class ComponentStatus(Enum):
    """组件状态枚举"""
    UNKNOWN = "unknown"
    INITIALIZING = "initializing"
    RUNNING = "running"
    ERROR = "error"
    STOPPED = "stopped"
    DEGRADED = "degraded"


@dataclass
class StatusInfo:
    """状态信息"""
    status: ComponentStatus
    message: str = ""
    timestamp: float = field(default_factory=time.time)
    error_count: int = 0
    last_error: Optional[str] = None
    metadata: Dict[str, Any] = field(default_factory=dict)


class VisionStatusManager(Node):
    """视觉系统状态管理器"""
    
    def __init__(self):
        super().__init__('vision_status_manager')
        
        # 组件状态跟踪
        self.component_status = {
            'echokit_server': StatusInfo(ComponentStatus.UNKNOWN),
            'camera_system': StatusInfo(ComponentStatus.UNKNOWN),
            'vision_processor': StatusInfo(ComponentStatus.UNKNOWN),
            'coze_api': StatusInfo(ComponentStatus.UNKNOWN),
            'local_model': StatusInfo(ComponentStatus.UNKNOWN),
            'websocket_bridge': StatusInfo(ComponentStatus.UNKNOWN)
        }
        
        # 状态变化回调
        self.status_callbacks: List[Callable[[str, StatusInfo], None]] = []
        
        # 错误传播配置
        self.error_propagation_rules = {
            'echokit_server': ['websocket_bridge'],
            'camera_system': ['vision_processor'],
            'vision_processor': [],
            'coze_api': ['vision_processor'],
            'local_model': ['vision_processor'],
            'websocket_bridge': []
        }
        
        # 线程锁
        self.status_lock = threading.RLock()
        
        # QoS配置
        self.qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            depth=10
        )
        
        # ROS2发布者
        self.system_status_publisher = self.create_publisher(
            String, '/vision/system_status', self.qos_profile
        )
        self.component_status_publisher = self.create_publisher(
            String, '/vision/component_status', self.qos_profile
        )
        self.error_alert_publisher = self.create_publisher(
            String, '/vision/error_alert', self.qos_profile
        )
        
        # ROS2订阅者 - 监听各组件状态
        self.camera_status_subscriber = self.create_subscription(
            String, '/camera/status', self.camera_status_callback, self.qos_profile
        )
        self.vision_analysis_subscriber = self.create_subscription(
            String, '/vision_analysis', self.vision_analysis_callback, self.qos_profile
        )
        self.echokit_status_subscriber = self.create_subscription(
            String, '/echokit/status', self.echokit_status_callback, self.qos_profile
        )
        
        # 定时器 - 定期检查系统状态
        self.status_check_timer = self.create_timer(5.0, self.periodic_status_check)
        
        # 健康检查配置
        self.health_check_timeout = 30.0  # 30秒无响应视为异常
        self.max_error_count = 5  # 最大错误次数
        
        self.get_logger().info('Vision Status Manager initialized')
    
    def update_component_status(self, component: str, status: ComponentStatus, 
                              message: str = "", metadata: Dict[str, Any] = None):
        """更新组件状态"""
        with self.status_lock:
            if component not in self.component_status:
                self.get_logger().warn(f'Unknown component: {component}')
                return
            
            old_status = self.component_status[component].status
            
            # 更新状态信息
            self.component_status[component].status = status
            self.component_status[component].message = message
            self.component_status[component].timestamp = time.time()
            
            if metadata:
                self.component_status[component].metadata.update(metadata)
            
            # 如果状态变为错误，增加错误计数
            if status == ComponentStatus.ERROR:
                self.component_status[component].error_count += 1
                self.component_status[component].last_error = message
            
            # 状态变化日志
            if old_status != status:
                self.get_logger().info(f'Component {component} status changed: {old_status.value} -> {status.value}')
                if message:
                    self.get_logger().info(f'  Message: {message}')
                
                # 触发状态变化回调
                self._trigger_status_callbacks(component, self.component_status[component])
                
                # 发布组件状态变化
                self._publish_component_status(component)
                
                # 处理错误传播
                if status == ComponentStatus.ERROR:
                    self._propagate_error(component, message)
                
                # 发布系统整体状态
                self._publish_system_status()
    
    def get_component_status(self, component: str) -> Optional[StatusInfo]:
        """获取组件状态"""
        with self.status_lock:
            return self.component_status.get(component)
    
    def get_system_status(self) -> Dict[str, Any]:
        """获取系统整体状态"""
        with self.status_lock:
            # 计算系统整体状态
            error_components = []
            degraded_components = []
            running_components = []
            
            for component, status_info in self.component_status.items():
                if status_info.status == ComponentStatus.ERROR:
                    error_components.append(component)
                elif status_info.status == ComponentStatus.DEGRADED:
                    degraded_components.append(component)
                elif status_info.status == ComponentStatus.RUNNING:
                    running_components.append(component)
            
            # 确定系统整体状态
            if error_components:
                if len(error_components) >= len(self.component_status) // 2:
                    overall_status = ComponentStatus.ERROR
                else:
                    overall_status = ComponentStatus.DEGRADED
            elif degraded_components:
                overall_status = ComponentStatus.DEGRADED
            elif running_components:
                overall_status = ComponentStatus.RUNNING
            else:
                overall_status = ComponentStatus.UNKNOWN
            
            return {
                'overall_status': overall_status.value,
                'error_components': error_components,
                'degraded_components': degraded_components,
                'running_components': running_components,
                'total_components': len(self.component_status),
                'timestamp': time.time(),
                'component_details': {
                    component: {
                        'status': info.status.value,
                        'message': info.message,
                        'timestamp': info.timestamp,
                        'error_count': info.error_count,
                        'last_error': info.last_error
                    }
                    for component, info in self.component_status.items()
                }
            }
    
    def add_status_callback(self, callback: Callable[[str, StatusInfo], None]):
        """添加状态变化回调"""
        self.status_callbacks.append(callback)
    
    def remove_status_callback(self, callback: Callable[[str, StatusInfo], None]):
        """移除状态变化回调"""
        if callback in self.status_callbacks:
            self.status_callbacks.remove(callback)
    
    def camera_status_callback(self, msg: String):
        """处理摄像头状态消息"""
        try:
            status_data = json.loads(msg.data)
            
            if status_data.get('type') == 'camera_status':
                connected = status_data.get('connected', False)
                if connected:
                    self.update_component_status(
                        'camera_system', 
                        ComponentStatus.RUNNING,
                        f"Camera connected: {status_data.get('resolution', 'unknown')}@{status_data.get('fps', 0)}fps",
                        status_data
                    )
                else:
                    self.update_component_status(
                        'camera_system',
                        ComponentStatus.ERROR,
                        "Camera disconnected",
                        status_data
                    )
            
        except Exception as e:
            self.get_logger().error(f'Error processing camera status: {e}')
    
    def vision_analysis_callback(self, msg: String):
        """处理视觉分析消息"""
        try:
            analysis_data = json.loads(msg.data)
            
            # 更新视觉处理器状态
            processing_time = analysis_data.get('processing_time_ms', 0)
            model_used = analysis_data.get('model_used', 'unknown')
            
            if processing_time > 0:
                self.update_component_status(
                    'vision_processor',
                    ComponentStatus.RUNNING,
                    f"Analysis completed in {processing_time}ms using {model_used}",
                    {'processing_time': processing_time, 'model_used': model_used}
                )
                
                # 更新对应的AI服务状态
                if model_used == 'coze':
                    self.update_component_status(
                        'coze_api',
                        ComponentStatus.RUNNING,
                        f"Coze API responded in {processing_time}ms"
                    )
                elif model_used == 'local':
                    self.update_component_status(
                        'local_model',
                        ComponentStatus.RUNNING,
                        f"Local model processed in {processing_time}ms"
                    )
            
        except Exception as e:
            self.get_logger().error(f'Error processing vision analysis: {e}')
    
    def echokit_status_callback(self, msg: String):
        """处理EchoKit状态消息"""
        try:
            status_data = json.loads(msg.data)
            
            if status_data.get('type') == 'echokit_connection':
                connected = status_data.get('connected', False)
                if connected:
                    self.update_component_status(
                        'echokit_server',
                        ComponentStatus.RUNNING,
                        "Connected to EchoKit Server"
                    )
                    self.update_component_status(
                        'websocket_bridge',
                        ComponentStatus.RUNNING,
                        "WebSocket bridge active"
                    )
                else:
                    self.update_component_status(
                        'echokit_server',
                        ComponentStatus.ERROR,
                        "Disconnected from EchoKit Server"
                    )
                    self.update_component_status(
                        'websocket_bridge',
                        ComponentStatus.ERROR,
                        "WebSocket bridge inactive"
                    )
            elif status_data.get('type') == 'echokit_error':
                error_msg = status_data.get('error', 'Unknown error')
                self.update_component_status(
                    'echokit_server',
                    ComponentStatus.ERROR,
                    f"EchoKit error: {error_msg}"
                )
            
        except Exception as e:
            self.get_logger().error(f'Error processing EchoKit status: {e}')
    
    def periodic_status_check(self):
        """定期状态检查"""
        current_time = time.time()
        
        with self.status_lock:
            for component, status_info in self.component_status.items():
                # 检查组件是否长时间无响应
                time_since_update = current_time - status_info.timestamp
                
                if time_since_update > self.health_check_timeout:
                    if status_info.status == ComponentStatus.RUNNING:
                        self.update_component_status(
                            component,
                            ComponentStatus.DEGRADED,
                            f"No response for {time_since_update:.1f}s"
                        )
                
                # 检查错误次数是否过多
                if status_info.error_count > self.max_error_count:
                    if status_info.status != ComponentStatus.ERROR:
                        self.update_component_status(
                            component,
                            ComponentStatus.ERROR,
                            f"Too many errors ({status_info.error_count})"
                        )
    
    def _trigger_status_callbacks(self, component: str, status_info: StatusInfo):
        """触发状态变化回调"""
        for callback in self.status_callbacks:
            try:
                callback(component, status_info)
            except Exception as e:
                self.get_logger().error(f'Error in status callback: {e}')
    
    def _propagate_error(self, component: str, error_message: str):
        """传播错误到相关组件"""
        if component in self.error_propagation_rules:
            affected_components = self.error_propagation_rules[component]
            
            for affected_component in affected_components:
                self.update_component_status(
                    affected_component,
                    ComponentStatus.DEGRADED,
                    f"Affected by {component} error: {error_message}"
                )
        
        # 发布错误警报
        alert_msg = String()
        alert_msg.data = json.dumps({
            'type': 'error_alert',
            'component': component,
            'error': error_message,
            'timestamp': time.time(),
            'affected_components': self.error_propagation_rules.get(component, [])
        })
        self.error_alert_publisher.publish(alert_msg)
    
    def _publish_component_status(self, component: str):
        """发布组件状态"""
        status_info = self.component_status[component]
        
        status_msg = String()
        status_msg.data = json.dumps({
            'component': component,
            'status': status_info.status.value,
            'message': status_info.message,
            'timestamp': status_info.timestamp,
            'error_count': status_info.error_count,
            'last_error': status_info.last_error,
            'metadata': status_info.metadata
        })
        self.component_status_publisher.publish(status_msg)
    
    def _publish_system_status(self):
        """发布系统整体状态"""
        system_status = self.get_system_status()
        
        status_msg = String()
        status_msg.data = json.dumps(system_status)
        self.system_status_publisher.publish(status_msg)
    
    def reset_component_errors(self, component: str):
        """重置组件错误计数"""
        with self.status_lock:
            if component in self.component_status:
                self.component_status[component].error_count = 0
                self.component_status[component].last_error = None
                self.get_logger().info(f'Reset error count for component: {component}')
    
    def shutdown(self):
        """关闭状态管理器"""
        self.get_logger().info('Shutting down Vision Status Manager')
        
        # 停止定时器
        if hasattr(self, 'status_check_timer'):
            self.status_check_timer.cancel()
        
        # 清理回调
        self.status_callbacks.clear()


def main(args=None):
    """主函数"""
    rclpy.init(args=args)
    
    try:
        status_manager = VisionStatusManager()
        rclpy.spin(status_manager)
    except KeyboardInterrupt:
        print('Vision Status Manager interrupted by user')
    except Exception as e:
        print(f'Error in Vision Status Manager: {e}')
    finally:
        if 'status_manager' in locals():
            status_manager.shutdown()
        rclpy.shutdown()


if __name__ == '__main__':
    main()